from naoqi import ALProxy
import time
import cmath

IP = "192.168.1.107"
PORT = 9559


def posture():
    proxy = ALProxy("ALMotion", IP, PORT)
    proxy.wakeUp()
    names = 'Body'
    proxy.stiffnessInterpolation(names, 1.0, 1.0)
    proxy.wakeUp()
    proxy.setAngles("RShoulderPitch", 91.1 * cmath.pi / 180, 0.2)
    proxy.setAngles("LShoulderPitch", 91.1 * cmath.pi / 180, 0.2)
    time.sleep(1)

    proxy.setAngles("RShoulderRoll", -57.2 * cmath.pi / 180, 0.2)
    proxy.setAngles("LShoulderRoll", 57.2 * cmath.pi / 180, 0.2)
    time.sleep(1)

    proxy.setAngles("RElbowRoll", 83.7 * cmath.pi / 180, 0.2)
    proxy.setAngles("LElbowRoll", -83.7 * cmath.pi / 180, 0.2)
    time.sleep(1)

    proxy.setAngles("RElbowYaw", 0.1 * cmath.pi / 180, 0.2)
    proxy.setAngles("LElbowYaw", -0.1 * cmath.pi / 180, 0.2)
    time.sleep(1)

    proxy.setAngles("RWristYaw", 99.1 * cmath.pi / 180, 0.2)
    proxy.setAngles("LWristYaw", -99.1 * cmath.pi / 180, 0.2)
    time.sleep(1)

    proxy.setAngles("RHand", 0.8, 0.2)
    proxy.setAngles("LHand", 0.8, 0.2)
    time.sleep(2)
    proxy.setAngles("RHand", 0.7, 0.2)
    proxy.setAngles("LHand", 0.7, 0.2)
    # time.sleep(1)
    proxy.setAngles("RHand", 0.5, 0.2)
    proxy.setAngles("LHand", 0.5, 0.2)
    # time.sleep(1)
    proxy.setAngles("RHand", 0.3, 0.2)
    proxy.setAngles("LHand", 0.3, 0.2)
    # time.sleep(1)
    proxy.setAngles("RHand", 0.2, 0.2)
    proxy.setAngles("LHand", 0.2, 0.2)
    # time.sleep(1)'''
    proxy.setAngles("RHand", 0.1, 0.2)
    proxy.setAngles("LHand", 0.1, 0.2)
    leftArmEnable = False
    rightArmEnable = False
    proxy.setMoveArmsEnabled(leftArmEnable, rightArmEnable)
    proxy.rest()

    # proxy.setAngles("RElbowYaw",1.8,0.2)
    # proxy.setAngles("LElbowYaw",-1.8,0.2)
    # time.sleep(1)

    # proxy.setAngles("RElbowRoll",0.1,0.2)
    # proxy.setAngles("LElbowRoll",-0.1,0.2)
    # time.sleep(1)

    # proxy.setAngles("LWristYaw",-0.1,0.2)
    # proxy.setAngles("RWristYaw",0.1,0.2)
    # time.sleep(1)


    # proxy.setAngles("RHand",0.8,0.2)
    # proxy.setAngles("LHand",0.8,0.2)
    # time.sleep(1)
    # proxy.setAngles("RHand",0.7,0.2)
    # proxy.setAngles("LHand",0.7,0.2)
    # time.sleep(1)
    # proxy.setAngles("RHand",0.5,0.2)
    # proxy.setAngles("LHand",0.5,0.2)
    # time.sleep(1)
    # proxy.setAngles("RHand",0.3,0.2)
    # proxy.setAngles("LHand",0.3,0.2)
    # time.sleep(1)
    # proxy.setAngles("RHand",0.2,0.2)
    # proxy.setAngles("LHand",0.2,0.2)
    # time.sleep(1)
    # proxy.setAngles("RHand",0.1,0.2)
    # proxy.setAngles("LHand",0.1,0.2)
    # leftArmEnable = False
    # rightArmEnable = False
    # proxy.setMoveArmsEnabled(leftArmEnable, rightArmEnable)
if __name__ == '__main__':
    posture()